home *** CD-ROM | disk | FTP | other *** search
/ Aminet 6 / Aminet 6 - June 1995.iso / Aminet / misc / sci / RARS_Amiga_2.lha / RARS / ramdu2.cpp < prev    next >
Encoding:
C/C++ Source or Header  |  1995-03-31  |  4.7 KB  |  150 lines
  1. // This robot driver is by Randy Saint, February, 1995
  2. // adapted to ver. 0.39 3/6/95 by M. Timin
  3.  
  4. #include <string.h>
  5. #include <stdlib.h>
  6. #include <math.h>
  7. #include "car.h"
  8.  
  9. #define max(x,y) (((x)<(y))?(y):(x))
  10.  
  11. #define SPEED_CORN 0.5
  12. #define SHORT_CORN (PI/2.0)
  13. #define ACCEL_CORN (PI/3.0)
  14. #define ALPHA_CORN (PI/16.0)
  15.  
  16. extern const double CARWID;
  17. extern char* glob_name;
  18.  
  19. con_vec Ramdu2(situation s)
  20. {
  21.    const char name[] = "Ramdu2";
  22.    static int init_flag = 1;
  23.    con_vec result;
  24.    double alpha, vc;
  25.    double Accel_vc;
  26.    double width;                      // track width, feet
  27.    double cur_rad;                    // current radius
  28.    double nex_rad;                    // next radius
  29.    double nex_alpha;                  // alpha we'll need on the next turn
  30.    double dist_from_inside;           // distance from inside of turn
  31.    double nex_corn_spd;               // speed for next corner
  32.    double steer_gain = .18;
  33.    const double steer_damp = .48;
  34.    const double corn_con = 6.0;
  35.    static double corn_spd = 65.0;
  36.    static double acc_const = 85.0;
  37.    static int max_accel = 0;
  38.  
  39.    if(init_flag)  {
  40.       strcpy(glob_name, name);
  41.       init_flag = 0;
  42.       result.alpha = result.vc = 0;
  43.       return result;
  44.    }
  45.  
  46.    if(stuck(s.backward, s.v,s.vn, s.to_lft,s.to_rgt, &result.alpha,&result.vc))
  47.       return result;
  48.  
  49.  
  50. //  if(s.to_lft < 1e-5 && s.to_lft>-1e-5)   // PREVENTS A DIV BY ZERO
  51.   //   s.to_lft = .01;
  52.  
  53.  
  54.    width = s.to_lft + s.to_rgt;                        // find width of track
  55.    cur_rad = s.cur_rad;
  56.    nex_rad = s.nex_rad;
  57.  
  58.    if (nex_rad < 0.0) {
  59.     nex_rad += width;
  60.    }
  61.    if(cur_rad == 0.0 && nex_rad == 0.0)
  62.       corn_spd = 200.0;
  63.    else if(cur_rad == 0.0 && nex_rad != 0.0) {
  64.       corn_spd = max(40.0, corn_con * sqrt(nex_rad > 0.0 ? nex_rad : -nex_rad));
  65.       if (s.nex_len < SHORT_CORN)
  66.           corn_spd += corn_spd * SPEED_CORN * sqrt(SHORT_CORN-s.nex_len);
  67.    }
  68.    else if (cur_rad != 0.0) {
  69.       corn_spd = max(40.0, corn_con * sqrt(cur_rad > 0.0 ? cur_rad : -cur_rad));
  70.       if (s.cur_len < SHORT_CORN)
  71.           corn_spd += corn_spd * SPEED_CORN * sqrt(SHORT_CORN-s.cur_len);
  72.  
  73.       if (nex_rad != 0.0)
  74.         nex_corn_spd = max(40.0, corn_con * sqrt(nex_rad > 0.0 ? nex_rad : -nex_rad));
  75.       else
  76.         nex_corn_spd = 200.0;
  77.    }
  78.  
  79.    // prevent division by zero in code to follow:
  80.    if(s.to_lft < 1e-5 && s.to_lft > -1e-5)
  81.       s.to_lft = .01;
  82.    if(s.to_rgt < 1e-5 && s.to_rgt > -1e-5)
  83.       s.to_rgt = .01;
  84.  
  85.    nex_alpha = 0.0;
  86.    if(nex_rad > 0.0)   {
  87.       nex_alpha = steer_gain * (4 * (s.to_lft -
  88.                          (s.after_rad < 0.0 ? width/3 : 1.6*CARWID)
  89.                                         ) / width + .2 * width/s.to_rgt);
  90.       dist_from_inside = s.to_lft;
  91.    }
  92.    else if (nex_rad < 0.0) {
  93.       nex_alpha = -steer_gain * (4 * (s.to_rgt -
  94.                                (s.after_rad > 0.0 ? width/3 : 1.6*CARWID)
  95.                                              ) / width  + .2 * width/s.to_lft);
  96.       dist_from_inside = s.to_rgt;
  97.    }
  98.    if(cur_rad == 0) {
  99.       alpha = 0.0;
  100.       if (s.to_end < (width * (dist_from_inside/width)))
  101.           alpha = nex_alpha * (width-s.to_end)/width;
  102.  
  103.     }
  104.    else if(cur_rad > 0.0)   {
  105.       alpha = steer_gain * (4 * (s.to_lft -
  106.                          (nex_rad < 0.0 ? width/3 : 1.6*CARWID)
  107.                                         ) / width + .2 * width/s.to_rgt);
  108.       if ((s.to_end) < ALPHA_CORN)  // we're coming out of a turn
  109.          alpha = alpha*(s.to_end/ALPHA_CORN) + nex_alpha*((ALPHA_CORN-s.to_end)/ALPHA_CORN);  // Scale alpha down to nex_alpha
  110.    }
  111.    else  {
  112.       alpha = -steer_gain * (4 * (s.to_rgt -
  113.                                (nex_rad > 0.0 ? width/3 : 1.6*CARWID)
  114.                                              ) / width  + .2 * width/s.to_lft);
  115.       if ((s.to_end) < ALPHA_CORN)  // we're coming out of a turn
  116.          alpha = alpha*(s.to_end/ALPHA_CORN) + nex_alpha*((ALPHA_CORN-s.to_end)/ALPHA_CORN);  // Scale alpha down to nex_alpha
  117.    }
  118.  
  119.    alpha -= steer_damp * s.vn / s.v;  // This is damping, to prevent oscillation
  120.  
  121.    if (((s.power_req < 0.85) && (max_accel)) || ((s.power_req > 1.0) && (max_accel))) {
  122.       if(s.power_req < 1e-5 && s.power_req > -1e-5)   // prevent div by 0
  123.          s.power_req = .001;
  124.         acc_const /= s.power_req;
  125.         acc_const *= 0.9;
  126.    }
  127.    Accel_vc = s.v + acc_const/s.v;
  128.    if(cur_rad == 0)         // If we are on a straightaway,
  129.       if((1.25*s.to_end/(s.v*0.05)) > (s.v - corn_spd))
  130.          vc = Accel_vc;        // keep accellerating near full power
  131.       else  {                 // otherwise,
  132.          vc = ((Accel_vc<corn_spd)? (Accel_vc) : corn_spd);
  133.       }
  134.    else {
  135.       if ((s.to_end) > ACCEL_CORN)
  136.          vc = corn_spd;
  137.       else
  138.          vc = corn_spd*(s.to_end/ACCEL_CORN) + nex_corn_spd*((ACCEL_CORN-s.to_end)/ACCEL_CORN);
  139.  
  140.       vc = ((Accel_vc<vc)? (Accel_vc) : vc);           // maintain cornering speed, braking at first
  141.  
  142.    }
  143.    if (vc == Accel_vc) max_accel = 1;
  144.    else max_accel = 0;
  145.  
  146.    result.vc = vc;   result.alpha = alpha;
  147.    return result;
  148. }                                                              // v;
  149.  
  150.